#include <stdio.h>
#include <stdlib.h>
#include "improve_div.h"


void AddVectorToVector(elem* ResultVector, elem* AddedVector, int iVecLength)
{
	int i;
	for (i = 0; i < iVecLength; i++)
		ResultVector[i] += AddedVector[i];
}

int Mult_ModMat_Vector(sparse_matrix_lst* A_g,int* GroupVertexDegrees,int M,int iGroupSize,elem* X,elem* ResultVec)
{
	int i;
	elem dTemp;
	elem *TempVector;

	TempVector = (elem*)calloc(iGroupSize,sizeof(elem));
	if (TempVector == NULL)
	{
		/*exit safely by signaling calling function to free mem and exit*/
		return 0;
	}
	/*after this call ResultVec will contain A_g(x)*/
	mult_sparse_list(A_g,X,&ResultVec);
	/* following formula of section 4.2 */
	dTemp = -DotProduct(GroupVertexDegrees,X,iGroupSize) / M;

	for (i = 0; i < iGroupSize; i++)
		TempVector[i] = dTemp * GroupVertexDegrees[i];
	AddVectorToVector(ResultVec,TempVector,iGroupSize);

	free(TempVector);

	return 1;
}

elem DotProduct(int* VectorA, elem* VectorB, int iLength)
{
	int i;
	elem dResult = 0;

	for (i=0; i<iLength; i++)
	{
		dResult += VectorA[i] * VectorB[i] ;
	}

	return dResult;
}

int FindImprovement(elem** B_g,sparse_matrix_lst* A_g,int iGroupSize,int* GroupVertexDegrees,int M,elem* vector_S,elem* Score,int* Unmoved,elem* Improve,int* Indices)
{
	int i,k;
 
	/* compute the initial score */
	InitialScore(A_g,GroupVertexDegrees,M,vector_S,Score,iGroupSize);
	for (i=0;i<iGroupSize;i++)
	{
		/*update score only from second iteration and on*/
		if (i!=0)
			UpdateScore(Score,B_g,vector_S,k,iGroupSize);
		/*Moving vertex k with a maximal score (line 11)*/
		k = UnmovedArgmax(Score,iGroupSize,Unmoved);
		vector_S[k]=(-vector_S[k]);
		Indices[i] = k;
		if (i==0)
			Improve[i] = Score[k];
		else
			Improve[i] = Improve[i-1] + Score[k];
		/*remove k from the unmoved vertex group (represented by -1 flag)*/
		Unmoved[k]=(-1);
	}

	return 1;
}

int FindMaxImprovement(elem* vector_s,int iGroupSize,elem* Score,elem* Improve,int* Indices,elem* pDeltaQ)
{
	int i,j,iMaxIndex;
	/*Find the maximum improvement of s and update s accordingly (line 21)*/
	iMaxIndex = ImproveArgmax(Improve,iGroupSize);
	/*line 22 of alg 4*/
	for (i=iGroupSize-1; i>iMaxIndex; i--)
	{
		j=Indices[i];
		vector_s[j]=(-vector_s[j]);
	}
	/*if iMaxIndex = group size -1, end process by setting deltaQ to 0 */
	if (iMaxIndex==(iGroupSize-1))
		(*pDeltaQ)=0;
	else
		(*pDeltaQ)=Improve[iMaxIndex];
	
	return 1;
}

 /* improves a given division of g */
int ImproveDivision(int *GroupVertexDegrees,int M, sparse_matrix_lst* A_g,elem** B_g,elem* vector_S,elem* pDeltaQ, int* Group,int iOriginalGroupLength, int* FirstSubGroup,int iSubGroupLength, int *numOfIter)
{
	int i, iIteration = 0;
	int *Unmoved = NULL, *Indices = NULL;
	elem *Score = NULL,*Improve = NULL;

	Unmoved = (int*) malloc(iOriginalGroupLength*sizeof(int));
	if (Unmoved == NULL)
	{
		ExitSafely1(GroupVertexDegrees,Unmoved,Indices,Score,Improve); 
		return 0;
	}
	Indices = (int*) malloc(iOriginalGroupLength*sizeof(int));
	if (Indices == NULL)
	{
		ExitSafely1(GroupVertexDegrees,Unmoved,Indices,Score,Improve);
		return 0;
	}
	Score =(elem*) malloc(iOriginalGroupLength*sizeof(elem));
	if (Score == NULL)
	{
		ExitSafely1(GroupVertexDegrees,Unmoved,Indices,Score,Improve);
		return 0;
	}
	Improve =(elem*) malloc(iOriginalGroupLength*sizeof(elem));
	if (Improve == NULL)
	{
		ExitSafely1(GroupVertexDegrees,Unmoved,Indices,Score,Improve);
		return 0;
	}

	/*implementation of algorithm 4*/
	
	/*main loop of alg 4 - until deltaQ is not positive*/
	do
	{
		for (i=0;i<iOriginalGroupLength;i++)
			Unmoved[i]=i;	  
		FindImprovement(B_g,A_g,iOriginalGroupLength,GroupVertexDegrees,M,vector_S,Score,Unmoved,Improve,Indices);
		FindMaxImprovement( vector_S,iOriginalGroupLength,Score,Improve,Indices,pDeltaQ);
		iIteration++;
	}
	while ((IS_POSITIVE(*pDeltaQ)) && (iIteration < MAX_ITTERATION));

	*numOfIter = iIteration;//@@@

	if (iIteration >= MAX_ITTERATION)  /*broke the loop because exceeded iteration limit*/
	{
		printf("convergence error: reached maximum number of permitted iterations.");
		ExitSafely1(GroupVertexDegrees,Unmoved,Indices,Score,Improve);
		return 0;
	}   

	free(Improve);
	free(Score);
	free(Indices);
	free(Unmoved);

	return 1;
}


int InitialScore(sparse_matrix_lst* A_g,int* GroupVertexDegrees,int M, elem* vector_S, elem* Score,int iGroupSize)
{
	int i;
	elem *TempVec;

	/*allocating the vector that will store B[g]*S*/
	TempVec = (elem *)(calloc(iGroupSize, sizeof(elem)));
	if(TempVec == NULL)
	{
		/*returning 0 will indicate calling functions to recursively free mem & exit safely   */
		return 0;
	}
	/*compute B[g]*S as required for the "fast" implementation of alg 4*/
	if (Mult_ModMat_Vector(A_g,GroupVertexDegrees,M,iGroupSize,vector_S,TempVec) == 0)
	{
		ExitSafely2(TempVec);
		return 0;
	}

	/*fill in Score*/
	for (i=0;i<iGroupSize;i++)
	{
		/*computing score according to formula (22)*/
		Score[i] = (-2)*((elem)vector_S[i]*TempVec[i] + (elem)((elem)(GroupVertexDegrees[i] * GroupVertexDegrees[i])/M)); 

	}

	free(TempVec);

	return 1;
}

int UpdateScore(elem* Score,elem** B_g,elem* s,int k,int iGroupSize)
{
	int i;
	/*performing 2nd loop of Section 4.2 (Fast implementation of score computations) */
	for (i=0;i<iGroupSize;i++)
		if (i == k)
			Score[i]=(-Score[i]);
		else
			Score[i]=(Score[i]-4*s[i]*s[k]*B_g[i][k]);

	return 1;
}


int UnmovedArgmax(elem *Score,int iVectorLength,int* Unmoved)
{
	int j,iMaxIndex=0;
	elem dCurrentMax=0;
	int iFlag = 0;
	/*for all indices i where Unmoved[i] = i (i.e i hasn't been moved yet), update Current-Max (if it is bigger than the old one) */
	for (j=0; j<iVectorLength; j++)
	{
		if (((iFlag  == 1) && (Score[j]>dCurrentMax)&&(Unmoved[j]==j)) ||
			((iFlag == 0) && (Unmoved[j]==j)))
		{
			iFlag = 1;
			dCurrentMax=Score[j];
			iMaxIndex=j;
		}
	}
	return iMaxIndex;
}

int ImproveArgmax(elem *Improve,int iVectorLength)
{
	int i,iMaxIndex=0;
	elem dCurrentMax=0;

	dCurrentMax = Improve[0];
	for (i=1;i<iVectorLength;i++)
		if (Improve[i]>dCurrentMax)
		{
			dCurrentMax=Improve[i];
			iMaxIndex = i;
		} 
	return iMaxIndex;
}

int FileToArray(char* sFileName, int* iGroupLength, int** Group_ptr)
{
	FILE*	file;
	int		iTemp, i;

	*iGroupLength = 0;

	/*open file for reading*/
	file = fopen(sFileName, "r"); 
	if (file == NULL)
	{
		/*free mem + close file*/
		ExitSafely3((*Group_ptr),file);
		return 0;
	}

	/*verify file not empty (empty group)*/
	while (fscanf(file, "%d", &iTemp) != EOF)
	{
		(*iGroupLength)++;
	}

	/*bringing cursor to beggining of file, to read values into array*/
	rewind(file);

	(*Group_ptr) = (int *)(calloc((*iGroupLength), sizeof(int)));
	if ((*Group_ptr) == NULL)
	{
		/*free mem + close file*/
		ExitSafely3((*Group_ptr),file);
		return 0;
	}

	i=0;
	while (fscanf(file, "%d", &iTemp) == 1)  
	{
		(*Group_ptr)[i] = iTemp ;  
		i++;
	}

	fclose(file);

	return 1;
}

int IsSubsetGroup(int* Group, int iGroupLength, int* SubGroup, int iSubGroupLength,elem** Vector_S_ptr)
{
	int iSubGroupIterator = 0,iGroupIterator = 0;
	if (iGroupLength < iSubGroupLength)
	{
		ExitSafely2((*Vector_S_ptr)); 
		return 0;
	}

	/*allocate the division vector S according to group size*/
	(*Vector_S_ptr)= (elem *)(calloc(iGroupLength, sizeof(elem)));
	if ((*Vector_S_ptr) == NULL)
	{
		ExitSafely2((*Vector_S_ptr)); 
		return 0;
	}

	iGroupIterator=0;
	for (iGroupIterator=0; iGroupIterator < iGroupLength ; iGroupIterator++)
	{
		if (Group[iGroupIterator] != SubGroup[iSubGroupIterator])
		{
			(*Vector_S_ptr)[iGroupIterator] = -1;      /*current vertex belongs to second group - mark as -1*/
		}
		else  /* Group[iGroupIterator] = SubGroup[iSubGroupIterator] */
		{
			(*Vector_S_ptr)[iGroupIterator] = 1;      /*current vertex belongs to first group - mark as 1*/
			iSubGroupIterator++;                      /*advance to next element in sub group*/
		}
	}

	if (iSubGroupIterator != iSubGroupLength)
	{
		return 0;
		}

	return 1;
}

int CalculateNewDivisionGroups(sparse_matrix_lst* A, elem** B_g, int* GroupVertexDegrees, int M, int iGroupSize, int* OriginalGroup, int *OriginalSubGroup, elem* vector_S,
					 int iOriginalSubGroupSize,int **ImprovedGroup1_ptr, int* pNewGroup1_Size, int **ImprovedGroup2_ptr, int* pNewGroup2_Size, elem* pDeltaQ, int *numOfIter)
{
	int i, iGroup_1_Iterator = 0, iGroup_2_Iterator = 0;
	elem iInitialQ= 0, iFinalQ = 0;
	sparse_matrix_lst* A_g = NULL;
	elem* F_g = NULL;
	elem **GnrModMat = NULL;

	if (GetAgFromA(A,&A_g,OriginalGroup,iGroupSize) == 0)
	{
		ExitSafely4(A_g,F_g,GnrModMat,iGroupSize);
		return 0;
	}

	if(CreateGeneralizedModMat(B_g,OriginalGroup,iGroupSize,&GnrModMat,&F_g) == 0)
	{
		ExitSafely4(A_g,F_g,GnrModMat,iGroupSize);
		return 0;
	}

	/* get the delta Q of the INITIAL division */
	if ( calcDeltaQ(A_g,GnrModMat,GroupVertexDegrees,F_g,M,vector_S,&iInitialQ) == 0 )
	{
		ExitSafely4(A_g,F_g,GnrModMat,iGroupSize);
		return 0;
	}

	/* improve the division */
	if (ImproveDivision(GroupVertexDegrees,M,A_g,B_g,vector_S,pDeltaQ,OriginalGroup,iGroupSize,OriginalSubGroup,iOriginalSubGroupSize, numOfIter) == 0)
	{
		ExitSafely4(A_g,F_g,GnrModMat,iGroupSize);
		return 0;
	}

	/* get the delta Q of the IMPROVED division */
	if ( calcDeltaQ(A_g,GnrModMat,GroupVertexDegrees,F_g,M,vector_S,&iFinalQ) == 0 )
	{
		ExitSafely4(A_g,F_g,GnrModMat,iGroupSize);
		return 0;
	}
	
	/*resetting the first new group size*/
	*pNewGroup1_Size = 0;  
	/*calculate size of first new group.S[0] is by definition in the 1st group, and so is any vertex who has the same sign as S[0]	  */
	for (i=0;i < iGroupSize; i++)
	{
		if (vector_S[i]*vector_S[0] > 0)
			(*pNewGroup1_Size)++;
	}

	/*calculating improvement*/
	(*pDeltaQ) = iFinalQ - iInitialQ;

	(*ImprovedGroup1_ptr) =(int*)calloc((*pNewGroup1_Size),sizeof(elem));
	if ((*ImprovedGroup1_ptr) == NULL)
	{
		ExitSafely4(A_g,F_g,GnrModMat,iGroupSize);
		return 0;
	}

	(*pNewGroup2_Size) = iGroupSize - (*pNewGroup1_Size);
	(*ImprovedGroup2_ptr)  =(int *)calloc((*pNewGroup2_Size),sizeof(elem));
	if ((*ImprovedGroup2_ptr) == NULL)
	{
		ExitSafely4(A_g,F_g,GnrModMat,iGroupSize);
		return 0;
	}

	/*fill in new (improved) groups*/
	for (i=0;i < iGroupSize; i++)
	{
		if (vector_S[i]*vector_S[0] > 0){   /*vertex belongs to 1st group*/
			(*ImprovedGroup1_ptr)[iGroup_1_Iterator] = OriginalGroup[i];
			iGroup_1_Iterator++;
		}
		else{								/*vertex belongs to 2nd group*/
			(*ImprovedGroup2_ptr)[iGroup_2_Iterator] = OriginalGroup[i];
			iGroup_2_Iterator++;
		}
	}

	Free2dimensionalArray(GnrModMat,iGroupSize);
	free_sparse_matrix_lst(A_g);
	free(F_g);
	/*calling function is responsible for the rest of memory freeing */

	return 1;
}

void ExitSafely1(int *GroupVertexDegrees,int *Unmoved, int *Indices,elem *Score, elem *Improve)
{
	if (GroupVertexDegrees != NULL)
		free(GroupVertexDegrees);
	if (Unmoved != NULL)
		free(Unmoved);
	if (Indices != NULL)
		free(Indices);
	if (Score != NULL)
		free(Score);
	if (Improve != NULL)
		free(Improve);
}

void ExitSafely2(elem* Arr)
{	
	if (Arr != NULL)
		free(Arr);
}

void ExitSafely3(int* Arr, FILE* fp)
{	
	if (Arr != NULL)
		free(Arr);
	
	fclose(fp);
}

void ExitSafely4(sparse_matrix_lst* A_g,elem* F_g,elem **GnrModMat,int iGroupSize)
{
	if (A_g != NULL)
		free_sparse_matrix_lst(A_g);
	if (F_g != NULL)
		free(F_g);
	if (GnrModMat != NULL)
		Free2dimensionalArray(GnrModMat,iGroupSize);
}

void ExitSafely5(sparse_matrix_lst* A,elem** B_g,elem* vector_S,int *NewGroup1, int*NewGroup2, int iGroupSize,int *OriginalGroup,int *OriginalSubGroup,int *GroupVertexDegrees)
{
	if(A != NULL)
		free_sparse_matrix_lst(A);
	if(B_g != NULL)
		Free2dimensionalArray(B_g,iGroupSize);
	if(OriginalGroup != NULL)
		free(OriginalGroup);
	if(OriginalSubGroup != NULL)
		free(OriginalSubGroup);
	if(GroupVertexDegrees != NULL)
		free(GroupVertexDegrees);
	if(vector_S != NULL)
		free(vector_S);
	if(NewGroup1 != NULL)
		free(NewGroup1);
	if(NewGroup2 != NULL)
		free(NewGroup2);
}

